#include <arch/psci.h>
#include <core/dprintf.h>
#include "tools/macros.h"

typedef uintptr_t(psci_fn)(uintptr_t, uintptr_t, uintptr_t, uintptr_t);
static psci_fn *invoke_psci_fn;

enum psci_function {
    PSCI_FN_CPU_SUSPEND,
    PSCI_FN_CPU_ON,
    PSCI_FN_CPU_OFF,
    PSCI_FN_MIGRATE,
    PSCI_FN_MAX,
};

static uint32_t psci_function_id[PSCI_FN_MAX];

static uintptr_t invoke_psci_fn_smc(uintptr_t function_id, uintptr_t arg0, uintptr_t arg1,
                                          uintptr_t arg2)
{
    struct arm_smccc_res res;
    
    do_smc(function_id, arg0, arg1, arg2, &res);
    return res.a0;
}

static uint32_t psci_get_version(void)
{
    return invoke_psci_fn(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0);
}

int32_t psci_cpu_on(uintptr_t cpuid, uintptr_t entry_point)
{
    int32_t      err;
    uint32_t fn;

    fn = psci_function_id[PSCI_FN_CPU_ON];

    err = invoke_psci_fn(fn, cpuid, entry_point, 0);

    return err;
}

static void psci_0_2_set_functions(void)
{
    dprintf("Using standard PSCI v0.2 function IDs");

    psci_function_id[PSCI_FN_CPU_ON] = PSCI_0_2_FN64_CPU_ON;

    psci_function_id[PSCI_FN_CPU_SUSPEND] = PSCI_0_2_FN64_CPU_SUSPEND;
    psci_function_id[PSCI_FN_CPU_OFF]     = PSCI_0_2_FN_CPU_OFF;
    psci_function_id[PSCI_FN_MIGRATE]     = PSCI_0_2_FN64_MIGRATE;
}

static int32_t psci_probe(void)
{
    uint32_t ver = psci_get_version();

    dprintf("PSCIv%d.%d detected in firmware.", PSCI_VERSION_MAJOR(ver), PSCI_VERSION_MINOR(ver));

    if (PSCI_VERSION_MAJOR(ver) == 0 && PSCI_VERSION_MINOR(ver) < 2) {
        dprintf("Conflicting PSCI version detected.");
        return -1;
    }

    psci_0_2_set_functions();

    return 0;
}

int32_t psci_init(void)
{
    invoke_psci_fn = invoke_psci_fn_smc;    // virt use psci v1.1 , method = smc

    int32_t err = psci_probe();
    if (err) {
        dprintf("psci probe failed");
        return err;
    }

    return 0;
}

